﻿#include "EquSolver.h"
#include <iostream>
#include <algorithm>
using namespace std;

bool SolveEqu_GuassColoumMainEle(std::vector<std::vector<double>>  A, int dim, std::vector<double>  b, std::vector<double> & x)
{
	try
	{
		x.resize(b.size(),0);
		for (int k = 0; k < dim - 1; k++)
		{
			//找到列主元row
			double mainEle = 0;
			int mainEleRow = -1;
			for (int j = k; j < dim; j++)
			{
				if (mainEle < fabs(A[j][k]))
				{
					mainEle = fabs(A[j][k]);
					mainEleRow = j;
				}
			}
			cout << "k=0 mainEleRow=" << mainEleRow << " val=" << A[mainEleRow][k] << endl;

			if (mainEleRow == -1)
			{
				cout << "solve equ:A is singular" << endl;
				return false;
			}

			//交换主元到对角线上
			for (int j = k; j < dim; j++)
			{
				std::swap(A[k][j], A[mainEleRow][j]);
			}

			std::swap(b[k], b[mainEleRow]);
			for (int i = k + 1; i < dim; i++)
			{
				double m_ik = A[i][k] / A[k][k];
				for (int j = k + 1; j < dim; j++)
				{
					A[i][j] = A[i][j] - m_ik * A[k][j];
				}
				b[i] = b[i] - m_ik * b[k];
			}
		}

		//
		x[dim - 1] = b[dim - 1] / A[dim - 1][dim - 1];
		for (int k = dim - 2; k >= 0; k--)
		{
			double temp = 0;
			for (int j = k + 1; j < dim; j++)
			{
				temp += (A[k][j] * x[j]);
			}
			x[k] = (b[k] - temp) / A[k][k];
		}
	}
	catch (std::exception & e)
	{
		cout << "slove equ exception happened:" << e.what() << endl;
		return false;
	}
	return true;
}

bool SolveEqu_GuassSeidel(std::vector<std::vector<double>>  A, int dim, std::vector<double>  b, std::vector<double> & x, double omega)
{
	//先看是否满足先决条件
	bool pre_require_row = true;
	bool pre_require_col = true;
	for (int k = 0; k < dim; k++)
	{
		double temp = 0;
		for (int j = 0; j < dim; j++)
		{
			if (j == k)
				continue;
			else
			{
				temp += fabs(A[k][j]);
			}
		}
		if (fabs(A[k][k]) < temp)
		{
			pre_require_row = false;
			break;
		}
	}
	for (int k = 0; k < dim; k++)
	{
		double temp = 0;
		for (int i = 0; i < dim; i++)
		{
			if (i == k)
				continue;
			else
			{
				temp += fabs(A[i][k]);
			}
		}
		if (fabs(A[k][k]) < temp)
		{
			pre_require_col = false;
			break;
		}
	}

	if (!(pre_require_col&&pre_require_row))
	{
		cout << "不是按行或列占优的矩阵" << endl;
		return false;
	}

	//for (int k = 0; k < dim; k++)
	//{
	//	//判断第i行第k列元素与A[k][k]是否同符号，是就加上i行，不是就减去
	//	for (int i = k + 1; i < dim; i++)
	//	{
	//		double alpha = 1;
	//		if (A[k][k] * A[i][k] > 0)
	//		{
	//			alpha = 1.0;
	//		}
	//		else
	//		{
	//			alpha = -1.0;
	//		}
	//		for (int j = 0; j < dim; j++)
	//		{
	//			A[k][j] += (alpha*A[i][j]);
	//		}
	//		b[k] += (alpha*b[i]);
	//	}
	//}

	//


	//初始化，这样模一般小于1
	for (int i = 0; i < dim; i++)
	{
		x[i] = 1.0 / dim;
	}

	double epsilon = INT_MAX;

	

	while (epsilon > 1e-5)
	{
		epsilon = 0;
		for (int i = 0; i < dim; i++)
		{
			double temp = 0;
			double temp_pre = 0;
			for (int j = 0; j < i; j++)
			{
				temp_pre += (A[i][j]*x[j]/A[i][i]);
			}
			double temp_post = 0;
			for (int j = i+1; j < dim; j++)
			{
				temp_post += (A[i][j] * x[j] / A[i][i]);
			}
			
			temp = omega * (b[i] / A[i][i] - (1.0 - 1.0 / omega)*x[i] - temp_pre - temp_post);
			epsilon += fabs(x[i] - temp);
			x[i] = temp;
		}
	}
	return true;
}